#include "Vehicle.h"
#include "Maths.h"

#define NODE_RADIUS 5.0f

namespace Pathing
{
	Vehicle::Vehicle() : IObject()
	{
	}
	Vehicle::Vehicle( const Maths::Vector3D& position, const Maths::Vector3D& destination ) : m_sleeping(true)
	{
		m_position = position;
		m_destination = destination;
	}
	
	bool& Vehicle::Sleeping()
	{
		return m_sleeping;
	}
	const Maths::Vector3D Vehicle::Destination() const
	{
		return m_destination;
	}
	void Vehicle::Destination( const Maths::Vector3D& destination )
	{
		m_destination = destination;
		m_sleeping = false;
	}
	void Vehicle::Direction( Maths::Vector3D& direction )
	{
		if (m_sleeping)
		{
			direction = Maths::Vector3D::ZERO;
			return;
		}
		direction = (m_currentDestination - m_position.getObject());
		if (direction.MagnitudeSq() <= NODE_RADIUS)
		{
			if (m_path.size() > 0)
			{
				PopPathingPoint();
			} else
			{
				m_sleeping = true;
				direction = Maths::Vector3D::ZERO;
			}
		} else
		{
			direction.Normalize();
		}
	}
	const Maths::Vector3D Vehicle::PeekPathingPoint() const
	{
		Maths::Vector3D node = m_currentDestination;
		if (m_path.size() > 0)
			node = m_path.front();
		return node;
	}
	std::list<Maths::Vector3D>& Vehicle::Path()
	{
		m_sleeping = false;
		return m_path;
	}
	const Maths::Vector3D Vehicle::PopPathingPoint()
	{
		if (m_path.size() > 0)
		{
			m_sleeping = false;
			m_currentDestination = m_path.front();
			m_path.pop_front();
		}
		return m_destination;
	}
};